#include  "../include/zeus_s2_driver/zeus_s2_driver.h"
#include <exception>  


DRIVE_DATA  dirveDate_ros;

DriverPub::DriverPub():
last_twist_time_(0),
line_speed_percent(0),
angle_speed_percent(0),
queueFlag(0),
can_recv_flag(0),
rev_left_(0),
rev_left_1_(0),
rev_right_(0),
rev_right_1_(0),
rev_flag(0),
driver_count(0)
{
    printf("DirvePub init.\n");
#ifdef      NVIDIA_XAVIER
    can_init();
    printf("CAN init OK ! \n");
#else
    //初始化参数，严格参数二次开发函数库说明书。
    int can1_ok = 0;
    int can2_ok = 0;
	VCI_INIT_CONFIG config;
	config.AccCode=0;
	config.AccMask=0xFFFFFFFF;
    config.Filter=1;                                                //接收所有帧
    /*
        波特率: 
                    500 Kbps  [0x00  0x1C]  
                    1000kbps  [0x00  0x14]
    */
    config.Timing0=0x00;
    config.Timing1=0x14;
	config.Mode=0;
    int num=VCI_FindUsbDevice2(dev_list);		                    //获取设备信息，返回设备数量
    printf(">>find USBCAN device [%d]! \n", num);                   //指示程序已运行
    if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)                          //打开设备
    {
        printf(">>open device success!\n");                         //打开设备成功
        if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)              //读取设备序列号、版本等信息。
        {
            printf(">>Get VCI_ReadBoardInfo success!\n");
		    printf(">>Serial_Num:%s \n", pInfo.str_Serial_Num);
		    printf(">>hw_Type:%s \n", pInfo.str_hw_Type);
		    printf(">>Firmware Version:V %x.%x%x \n",
                    (pInfo.fw_Version&0xF00)>>8,
                    (pInfo.fw_Version&0xF0)>>4,
                    (pInfo.fw_Version&0xF)            
                );	
	    }
        else
	    {
	        printf(">>Get VCI_ReadBoardInfo error!\n");
		    exit(1);
	    }
    }
    else
    {
        printf(">>open device error!\n");
	    exit(1);
    }       
    if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
    {
        printf(">>Init CAN1 error\n");
	    VCI_CloseDevice(VCI_USBCAN2,0);
    }
    else
    {
        if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
        {
	        printf(">>Start CAN1 error\n");
	        VCI_CloseDevice(VCI_USBCAN2,0);
        }
    }

    if(VCI_InitCAN(VCI_USBCAN2,0,1,&config)!=1)
    {
        printf(">>Init can2 error\n");
	    VCI_CloseDevice(VCI_USBCAN2,0);
    }
    else
    {
        if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
	    {
	        printf(">>Start can2 error\n");
		    VCI_CloseDevice(VCI_USBCAN2,0);
	    }
    }
    printf("CAN init OK ! \n");
#endif
}

DriverPub::~DriverPub()
{
    boost::mutex::scoped_lock look(mutex_);
    l_wheel_mult_ = 0;
    r_wheel_mult_ = 0;
//高配版
#ifdef  NVIDIA_XAVIER
    can_deinit();
    memset(buffer_data,0,17);
#else       //低配版
    VCI_ClearBuffer(VCI_USBCAN2,0,0);   //清空缓存
    VCI_ClearBuffer(VCI_USBCAN2,0,1);
    VCI_ResetCAN(VCI_USBCAN2,0,0);      //复位通道
    VCI_ResetCAN(VCI_USBCAN2,0,1);
    VCI_CloseDevice(VCI_USBCAN2,0);     //关闭can分析仪
#endif
    printf("DirvePub deinit.\n");

}

void DriverPub::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

void DriverPub::cmd_joy_callback(const zeus_s2_msgs::joy_control::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        ps3_control_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

	
// 使能电机驱动指令
void DriverPub::enable_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x24;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 1;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) != 1)
	{
        printf("enable_motor_driver failed!\r\n");
        exit(1);
	}
#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ msg;
	msg.ID = driver_id;  //0x101  102  103  104
	msg.SendType=0;
	msg.RemoteFlag=0;
	msg.ExternFlag=0;		//标准帧
	msg.DataLen=8;
	
	msg.Data[0] = 0;		
	msg.Data[1] = 0x24;		//0x1E 写使能保存了
	msg.Data[2] = 0;		
	msg.Data[3] = 0x10;
	msg.Data[4] = 0;		
	msg.Data[5] = 1;		
	msg.Data[6] = 0;		
	msg.Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &msg, 1) != 1)
	{
		printf("enable_motor_driver failed!\r\n");
        exit(1);
	}

#endif
}

// 失能电机驱动指令
void DriverPub::disable_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x1E;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 0;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) != 1)
    {
        printf("disable_motor_driver failed!\r\n");
        exit(1);
	}
#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ msg;
	msg.ID = driver_id;  //0x101  102  103  104
	msg.SendType=0;
	msg.RemoteFlag=0;
	msg.ExternFlag=0;		//标准帧
	msg.DataLen=8;
	
	msg.Data[0] = 0;		
	msg.Data[1] = 0x1E;		//0x1E 写使能保存了
	msg.Data[2] = 0;		
	msg.Data[3] = 0x10;
	msg.Data[4] = 0;		
	msg.Data[5] = 0;		
	msg.Data[6] = 0;		
	msg.Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &msg, 1) != 1)
	{
		printf("disable_motor_driver failed!\r\n");
        exit(1);
	}
#endif
}

// 清除驱动器报警指令 失能----》清除
void DriverPub::clear_ALM_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x21;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 0;		
	tx_buff[6] = 0;		
	tx_buff[7] = 1;		
    if( can0_write(driver_id, tx_buff) != 1)
	{
		printf("clear_ALM_motor_driver failed!\r\n");
        exit(1);
	}
#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ msg;
	msg.ID = driver_id;  //0x101  102  103  104
	msg.SendType=0;
	msg.RemoteFlag=0;
	msg.ExternFlag=0;		//标准帧
	msg.DataLen=8;
	
	msg.Data[0] = 0;		
	msg.Data[1] = 0x21;		
	msg.Data[2] = 0;		
	msg.Data[3] = 0x10;
	msg.Data[4] = 0;		
	msg.Data[5] = 0;		
	msg.Data[6] = 0;		
	msg.Data[7] = 1;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &msg, 1) != 1)
	{
		printf("clear_ALM_motor_driver failed!\r\n");
        exit(1);
	}
#endif
}


//设置电机为转距模式
void DriverPub::Set_motor_torque_mode(uint16_t driver_id )
{
#ifdef      NVIDIA_XAVIER
	//需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x1E;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x03;
	tx_buff[4] = 0;		
	tx_buff[5] = 1;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) != 1)
	{
		printf("set_motor_torque_mode failed!\r\n");
        exit(1);
	}
 #else
    int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ msg;
	msg.ID = driver_id;  //0x101  102  103  104
	msg.SendType=0;
	msg.RemoteFlag=0;
	msg.ExternFlag=0;		//标准帧
	msg.DataLen=8;
	
	msg.Data[0] = 0;		
	msg.Data[1] = 0x1E;		
	msg.Data[2] = 0;		
	msg.Data[3] = 0x03;
	msg.Data[4] = 0;		
	msg.Data[5] = 1;		
	msg.Data[6] = 0;		
	msg.Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &msg, 1) != 1)
	{
		printf("set_motor_torque_mode failed!\r\n");
        exit(1);
	}
#endif
}

//设定电机的转距值
void DriverPub::Set_torque_value_to_motor(uint16_t driver_id ,uint16_t value )
{
#ifdef      NVIDIA_XAVIER
	tx_buf[0]  = 0;		
	tx_buf[1] = 0x28;		
	tx_buf[2] = 0;		
	tx_buf[3] = 0x01;
	tx_buf[4] = (value>>8)&0xFF;		
	tx_buf[5] = (value&0xFF);		
	tx_buf[6] = 0;		
	tx_buf[7] = 0;		
	if( can0_write(driver_id, tx_buf) != 1)
	{
		printf("set_torque_value_to_motor failed!\r\n");
        exit(1);
	}
#else
    int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ msg;
	msg.ID = driver_id;  //0x101  102  103  104
	msg.SendType=0;
	msg.RemoteFlag=0;
	msg.ExternFlag=0;		//标准帧
	msg.DataLen=8;
	
	msg.Data[0] = 0;		
	msg.Data[1] = 0x28;		
	msg.Data[2] = 0;		
	msg.Data[3] = 0x01;
	msg.Data[4] = (value>>8)&0xFF;		
	msg.Data[5] = (value&0xFF);		
	msg.Data[6] = 0;		
	msg.Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &msg, 1) != 1)
	{
		printf("set_torque_value_to_motor failed!\r\n");
        exit(1);
	}
#endif
}

void DriverPub::send_speed_callback()
{
    double linear_speed, angular_speed;
    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {
        linear_speed = current_twist_.linear.x;
        angular_speed = current_twist_.angular.z;
    } 
    else
    {
        linear_speed = 0;
        angular_speed = 0;
    }
	if(linear_speed == 0)
	{
        right = -angular_speed*wheel_track_ /2.0;
        left  = right;	
	}
	else
	{
		if(angular_speed > 0 && angular_speed < start_rotation_limit_w_ && (linear_speed > 0 && linear_speed < 0.1))
		{
			angular_speed = start_rotation_limit_w_;
		}
		if(angular_speed < 0 && angular_speed > -start_rotation_limit_w_ && (linear_speed > -0.1 && linear_speed < 0))
		{
			angular_speed = -start_rotation_limit_w_;
		}
        left  = (linear_speed - angular_speed * wheel_track_  / 2.0);
        right = -(linear_speed + angular_speed * wheel_track_ / 2.0);

	}
   
}

/*设定速度模式*/
void DriverPub::Set_motor_speed_mode(uint16_t driver_id )
{
#ifdef      NVIDIA_XAVIER

#else
    int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x1E;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x03;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 2;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) != 1)
	{
		printf("set_motor_speed_mode failed!\r\n");
	}
#endif
}

/*给定速度值*/
void DriverPub::Set_speed_value_to_motor(uint16_t driver_id ,uint16_t value )
{
#ifdef  NVIDIA_XAVIER

#else
    int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x28;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0;
	send[0].Data[4] = (value>>8)&0xFF;		
	send[0].Data[5] = (value&0xFF);		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) != 1)
	{
		printf("set_speed_value_to_motor failed!\r\n");
	}
#endif
}


//采集底盘编码器值
//              前
//      601 -----602
//    左         |        右     
//      604 -----603
//              后
//


void DriverPub::updateParam(int id, uint8_t *data)
{
    if(data[1]==0xCA){
        rev_left_ = ((data[2]<<24)|(data[3]<<16)|(data[4]<<8)|data[5]);     //编码器
        motorEleVol[id] = (data[6]<<8)|(data[7]);                           //电流值 千分单位
        motorEleVol[id] = motorEleVol[0]/1000.0;
        queueFlag |= 1<<id;
        rev_flag |= 1<<id;
    }
    else if(data[1]==0xCB)
    {
        motorTemperature[id] = (data[2]<<8)|(data[3]);                      //电机的温度
        motorActSpeed[id] = -((data[4]<<8)|(data[5]));                      //电机的速度
        driverFaultCode[id] = (data[6]<<8)|(data[7]);                       //驱动器故障码
    }   
}

void DriverPub::Calculate_LeftAndRight_Meter(void *handle)
{
#ifdef NVIDIA_XAVIER
    can_frame  p = *(can_frame  *)handle;
    uint8_t data[8];
    uint32_t id = p.can_id ;
    memcpy(data, p.data, 8);
#else
    VCI_CAN_OBJ p = *(VCI_CAN_OBJ *)handle;
    uint8_t data[8];
    uint32_t id = p.ID;
    memcpy(data, p.Data, 8);
#endif
	if((id >= DRIVER_ID_ONE_POSE_ACK)&&(id <= DRIVER_ID_FOUR_POSE_ACK)){
        updateParam((id-DRIVER_ID_ONE_POSE_ACK), data);
	}

    if(id == DRIVER_ID_MODE_ACK)
    {
        if(data[0]==0x01)
        {
            if((data[1]==0x25)&&(data[3]==0x10)&&(data[5]==01))
            {
                printf("1号电机使能成功\n");
            }              
        }
        else if(data[0]==0x02)
        {
            if((data[1]==0x25)&&(data[3]==0x10)&&(data[5]==01))
            {
                printf("2号电机使能成功\n");
            }               
        }
        else if(data[0]==0x03)
        {
            if((data[1]==0x25)&&(data[3]==0x10)&&(data[5]==01))
            {
                printf("3号电机使能成功\n");
            }            
        }
        else if(data[0]==0x04)
        {
            if((data[1]==0x25)&&(data[3]==0x10)&&(data[5]==01))
            {
                printf("4号电机使能成功\n");
            }     
        }  
    }  
}

/*判断模式切换是否成功
*参数：     
* isnot:    
*   1   进行判断
*   0   不判断
* mode  运行模式/驻车模式
*返回值：
*   1   成功
*   0   失败
*/
bool DriverPub::mode_switch_judge(void *handle,uint8_t mode,bool isnot)
{
#ifdef NVIDIA_XAVIER
    can_frame  p = *(can_frame *)handle;
    uint8_t data[8];
    uint32_t id = p.can_id ;
    memcpy(data, p.data, 8);
#else
    VCI_CAN_OBJ p = *(VCI_CAN_OBJ *)handle;
    uint8_t data[8];
    uint32_t id = p.ID;
    memcpy(data, p.Data, 8);
#endif
    if(!isnot){
        return 0;
    }
    switch (mode)
    {
        case DRIVER_OPERATION_MODE:
            if(id == DRIVER_ID_MODE_ACK)
            {
                if((data[0]==0x01)&&(data[1]==0x1F))
                {
                      /*答者ID 01   设置接收标志位*/
                      mode_rev_flag |= (1<<0);
                }
                else if((data[0]==0x02)&&(data[1]==0x1F))
                {
                     /*答者ID 02   设置接收标志位*/
                     mode_rev_flag |= (1<<1);
                }
                else if((data[0]==0x03)&&(data[1]==0x1F))
                {
                     /*答者ID 03   设置接收标志位*/
                      mode_rev_flag |= (1<<2);
                }
                else if((data[0]==0x04)&&(data[1]==0x1F))
                {
                     /*答者ID 04   设置接收标志位*/
                      mode_rev_flag |= (1<<3);
                }
                if(mode_rev_flag == 0x0F){
                    return true;
                }
              
            }
        break;
    case DRIVER_PARKING_MODE:
        break;
    default:
        break;
    }
    return 0;
}

uint8_t DriverPub::get_recvFlag(void)
{
	return can_recv_flag;

}

void DriverPub::handle_speed_data(uint16_t number)
{
    if(start_flag_)
    {
        accumulation_y_ = 0.0;
        accumulation_x_ = 0.0;
        accumulation_th_ = 0.0;

		accumulation_left_ = 0.0;
    	accumulation_right_ = 0.0;
		accumulation_right_1_ = 0.0;
		accumulation_left_1_ = 0.0;


        cur_left_ = rev_left_;
		cur_left_1_ = rev_left_1_;
        cur_right_= rev_right_;
		cur_right_1_ = rev_right_1_;

		delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;

        last_time_[0] = now_[0];
        last_time_[1] = now_[1];
        last_time_[2] = now_[2];
        last_time_[3] = now_[3];
        start_flag_ = false;
        ROS_INFO("ROBOT is Successfuly connected!");
        ROS_INFO_STREAM("The Code Version is  " << code_version_);

    }
     if(cur_left_ == 0 && cur_right_ == 0)
    {
        delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;
    }

    switch(number)
    {
        case DRIVER_ID_ONE_POSE_ACK:
                if((rev_flag & 0x01)==0)
                {
                    return;
                }
                now_[0] = ros::Time::now();
                // delta_time_[0] = (now_[0] - last_time_[0]).toSec();
  
                if(rev_left_< encoder_low_wrap_ && cur_left_ > encoder_high_wrap_)
                {
                    l_wheel_mult_++;
                }
                else if(rev_left_ > encoder_high_wrap_ && cur_left_ < encoder_low_wrap_)
                {
                    l_wheel_mult_--;
                }
                else
                {
                    l_wheel_mult_ = 0;
                }
                delta_left_ = 1.0*(rev_left_ + (l_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_left_))/ticks_per_meter_;                 //转换为米单位
                accumulation_left_ += (delta_left_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                cur_left_ = rev_left_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.front_left_encoder.velocity =  (motorActSpeed[0]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   /*转速/减速比*轮子直径*PI / 60    单位 速度 m/s*/
                rev_flag &= ~(1<<0);                                                                                                                           //清标志
        break;
        case DRIVER_ID_TWO_POSE_ACK:
                 if((rev_flag & 0x02) ==0)
                {
                    return;
                }
                now_[1] = ros::Time::now();
                if(rev_right_ < encoder_low_wrap_ && cur_right_ > encoder_high_wrap_)
                {
                    r_wheel_mult_++;
                }  
                else if(rev_right_ > encoder_high_wrap_ && cur_right_ < encoder_low_wrap_)
                {
                    r_wheel_mult_--;
                }    
                else
                {
                    r_wheel_mult_ = 0;
                }
                delta_right_ = -(1.0*(rev_right_ + (r_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_right_))/ticks_per_meter_);                 //转换为米单位
                accumulation_right_ += (delta_right_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                cur_right_ = rev_right_;                                                                                                              //保存当前编码器位置
               left_right_wheel_state_.front_right_encoder.velocity =  (motorActSpeed[1]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                rev_flag &= ~(1<<1);     
        break;
        case DRIVER_ID_THREE_POSE_ACK:
                 if((rev_flag & 0x04)==0)
                {
                    return;
                }
                now_[2] = ros::Time::now();
                 if(rev_right_1_ < encoder_low_wrap_ && cur_right_1_ > encoder_high_wrap_)
                {
                    r_wheel_mult_1_++;
                }  
                else if(rev_right_1_ > encoder_high_wrap_ && cur_right_1_ < encoder_low_wrap_)
                {
                    r_wheel_mult_1_--;
                }    
                else
                {
                    r_wheel_mult_1_ = 0;
                }
                delta_right_1_ = -(1.0*(rev_right_1_ + (r_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_right_1_))/ticks_per_meter_);                 //转换为米单位
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_right_1_ += (delta_right_1_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // left_right_wheel_state_.rear_right_joint.position  = accumulation_right_1_;    //用于发布的msg
                cur_right_1_ = rev_right_1_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.rear_right_encoder.velocity = (motorActSpeed[2]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                // printf("rear_right_encoder.velocity:%lf\n",left_right_wheel_state_.rear_right_encoder.velocity);
                // last_time_[2] = now_[2];
                rev_flag &= ~(1<<2);     
        break;
        case DRIVER_ID_FOUR_POSE_ACK:
                 if((rev_flag & 0x08)==0)
                {
                    return;
                }
                now_[3] = ros::Time::now();
                 if(rev_left_1_< encoder_low_wrap_ && cur_left_1_ > encoder_high_wrap_)
                {
                    l_wheel_mult_1_++;
                }
                else if(rev_left_1_ > encoder_high_wrap_ && cur_left_1_< encoder_low_wrap_)
                {
                    l_wheel_mult_1_--;
                }
                else
                {
                    l_wheel_mult_1_ = 0;
                }
                delta_left_1_ = 1.0*(rev_left_1_ + (l_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_left_1_))/ticks_per_meter_;                 //转换为米单位
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_left_1_ += (delta_left_1_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // left_right_wheel_state_.rear_left_joint.position  = accumulation_left_1_;    //用于发布的msg
                cur_left_1_ = rev_left_1_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.rear_left_encoder.velocity = (motorActSpeed[3]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                // printf("rear_left_encoder.velocity:%lf\n",left_right_wheel_state_.rear_left_encoder.velocity);
                // last_time_[3] = now_[3];
                rev_flag &= ~(1<<3);     
        break;
        default:
        break;

    }
}   
  

void DriverPub::receive_func()  //接收线程。
{
#ifdef    NVIDIA_XAVIER
	int reclen=0;
	int i,j;
    int ind=0;
	while(true){
        /*调用接收函数，如果有数据，进行数据处理显示。*/
		if(can0_read( &can_id)){	
					Calculate_LeftAndRight_Meter(&can_id);    /*采集上报的编码器、转速、电流等信息*/
                    mode_switch_judge(&can_id,switching_mode,mode_isnot);
                    handle_speed_data(can_id.can_id);                /*将接收的数据进行处理*/
		}
        else{
			// printf("接收超时\r\n");
		}

	}
	printf("run thread exit\n");//退出接收线程		
#else
	int reclen=0;	
	VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
	int i,j;
    int ind=0;

	while(true){
        /*调用接收函数，如果有数据，进行数据处理显示。*/
		if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0){
			for(j=0;j<reclen;j++){
                Calculate_LeftAndRight_Meter(&rec[j]);    /*采集上报的编码器、转速、电流等信息*/
                mode_switch_judge(&rec[j],switching_mode,mode_isnot);
                handle_speed_data(rec[j].ID);                /*将接收的数据进行处理*/
			}
		}
        else{
			// printf("接收超时\r\n");
		}

	}
	printf("run thread exit\n");//退出接收线程		
#endif
}

//初始化机器人串口
bool DriverPub::initRobot()
{
    for(int i=0; i<MOTOR_DRV_MAX; i++)
    {
        //设置电机失能模式
        disable_motor_driver(DRIVER_ID_ONE_CONTROL+i);
        //设置电机力矩模式  
        Set_motor_torque_mode(DRIVER_ID_ONE_CONTROL+i);
        //设定电机的力矩为零
        Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL+i, 0);
        //清除超时报警指令
        clear_ALM_motor_driver( DRIVER_ID_ONE_CONTROL+i);
    }
    sleep(1);

    // 设置使能电机驱动器
    enable_motor_driver(DRIVER_ID_ONE_CONTROL+0);
    enable_motor_driver(DRIVER_ID_ONE_CONTROL+1);
    enable_motor_driver(DRIVER_ID_ONE_CONTROL+2);
    enable_motor_driver(DRIVER_ID_ONE_CONTROL+3);
    
    return 1;

}

//PID控制
int16_t  DriverPub::motor_velocityPidCtl(int motor_id, float target_speed, float current_speed)
{
    static float motor_speed_out;
    static int32_t   temp = 0;
    bias[motor_id] = target_speed - current_speed;
    //抗积分饱和20200718
    if(motor_speed_out >= MAX)//正向超限
    {
        if(bias[motor_id] < 0)//只累加负偏差
        {
            bias_integral[motor_id] += bias[motor_id];  
        }
    }
    else if(motor_speed_out <= MIN)//反向超限
    {
        if(bias[motor_id] > 0)//只累加正偏差
        {
            bias_integral[motor_id] += bias[motor_id];  
        }
    }
    else
    {
        bias_integral[motor_id] += bias[motor_id];  
    }
    motor_speed_out = motor_kp[motor_id] * bias[motor_id] *PID_SCALE + 
                      motor_kd[motor_id] * (bias[motor_id]-bias_last[motor_id]) *PID_SCALE + 
                      motor_ki[motor_id] * bias_integral[motor_id] *PID_SCALE;

    if(motor_speed_out > MAX)  //2^14 * 0xFFFF
    {
    	motor_speed_out = MAX;
    }
    else if(motor_speed_out < MIN)
    {
    	motor_speed_out = MIN;
    }
                  
    bias_last[motor_id] = bias[motor_id];

    return (int16_t)motor_speed_out;

 }

//动态重配置功能回调函数
void DriverPub::dynamic_reconfig_callback(zeus_s2_driver::PID_reconfigConfig &config,uint32_t level)
{
	ROS_INFO("Set PID P:[%d], I:[%d], D:[%d], ELE:[%d]",config.Kp,config.Ki,config.Kd,config.Ele_curr);
    pid_wheel_ = config.Ele_curr;
    speed_l = config.Speed_l;
    speed_r = config.Speed_r;
    if(pid_wheel_ ==1)
    {
        motor_kp[0] = config.Kp;
        motor_ki[0] = config.Ki;
        motor_kd[0] = config.Kd;
    }else if(pid_wheel_ == 2){
        motor_kp[1] = config.Kp;
        motor_ki[1] = config.Ki;
        motor_kd[1] = config.Kd;
    }else if(pid_wheel_ == 3){
        motor_kp[2] = config.Kp;
        motor_ki[2] = config.Ki;
        motor_kd[2] = config.Kd;
    }else if(pid_wheel_ == 4){
        motor_kp[3] = config.Kp;
        motor_ki[3] = config.Ki;
        motor_kd[3] = config.Kd;
    }
    reset_vol = config.bool_param;
    mode_selection = config.mode_param;
}

//返回心跳计数值
uint64_t DriverPub::my_xTaskGetTickCount(void)
{
    return count;

}

//检测通信是否超时中断
//判断故障标志
void  DriverPub::motor_driver_check(void)
{
    if((ros::Time::now() - now_[0]).toSec() >= 1)
    {
        ROS_WARN_THROTTLE(1,"Didn't received odom data,Please check your connection!");
        printf("请检查急停按钮和驱动器电源按钮是否按下，如果按下，请先关闭此程序，然后打开急停开关和驱动器电源按钮并重新启动此程序\n");
        motorPreSpeed[0] = 0;
        motorPreSpeed[1] = 0;
        motorPreSpeed[2] = 0;
        motorPreSpeed[3] = 0;
		driver_count = 11;
    }
    for(int i=0;i<4;i++){
        if(driverFaultCode[i]!=0)
        {
            printf("driverFaultCode[%d]:%d\n",i,driverFaultCode[i]);
            driver_count++;
        }else{
            // driver_count = 0;
        }
    }
    
    if(driver_count>10)
    {
        printf("驱动器故障警告，停止任务\n");
        ros::shutdown();
        exit(1);
    }

    if(count > 200)            //8ms *200 = 1600ms
    {
        if(queueFlag != 0x0F)
        {
            while(ros::ok())
            {
                printf("电机驱动器失联，请检查驱动器硬件通讯连接\n");
                motorPreSpeed[0] = 0;
                motorPreSpeed[1] = 0;
                motorPreSpeed[2] = 0;
                motorPreSpeed[3] = 0;
                /*发送电机转距值*/
                Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,motorPreSpeed[0] );
                Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,motorPreSpeed[1] );
                Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,motorPreSpeed[2] );
                Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,motorPreSpeed[3] );
                printf("停止任务\n");
                ros::shutdown();
            }
                
        }
        queueFlag = 0;
        count = 0;
    }
}

//主循环
void DriverPub::loop()
{

    ros::NodeHandle n("~");
    ros::Rate loop_rate(125);

    //获取配置参数
    n.param<std::string>("version",code_version_,std::string("V1.0.0"));

    n.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    n.param<double>("encoder_resolution_4wd",encoder_resolution_4wd_,10000);
    n.param<double>("wheel_track_4wd",wheel_track_4wd_,0.56);  
    n.param<double>("wheel_track",wheel_track_,0.56);  
    n.param<double>("linear_correction_factor",linear_correction_factor_,1.0);
    n.param<double>("angular_correction_factor",angular_correction_factor_,1.0);
    n.param<double>("gear_reduction_4wd",gear_reduction_4wd_,40);
    n.param<double>("wheel_diameter_4wd",wheel_diameter_4wd_,0.305);
    n.param<double>("start_rotation_limit_w",start_rotation_limit_w_,0.4);								//角速度限速
    n.param<bool>("start_flag",start_flag_,true);
    n.param<int>("encoder_min",encoder_min_,-2147483648);
    n.param<int>("encoder_max",encoder_max_,2147483647);
    n.param<int>("l_wheel_mult",l_wheel_mult_,0);
    n.param<int>("r_wheel_mult",r_wheel_mult_,0);
    n.param<float>("Kp_0",motor_kp[0],0.0);
    n.param<float>("Ki_0",motor_ki[0],0.0);
    n.param<float>("Kd_0",motor_kd[0],0.0);

     n.param<float>("Kp_1",motor_kp[1],0.0);
    n.param<float>("Ki_1",motor_ki[1],0.0);
    n.param<float>("Kd_1",motor_kd[1],0.0);

     n.param<float>("Kp_2",motor_kp[2],0);
    n.param<float>("Ki_2",motor_ki[2],0);
    n.param<float>("Kd_2",motor_kd[2],0);

     n.param<float>("Kp_3",motor_kp[3],0);
    n.param<float>("Ki_3",motor_ki[3],0);
    n.param<float>("Kd_3",motor_kd[3],0);

    n.param<int>("Ele_curr",ele_currVol,0);
    n.param<float>("Speed_l",speed_l,0);
    n.param<float>("Speed_r",speed_r,0);

    encoder_low_wrap_ = ((encoder_max_-encoder_min_)*0.3 + encoder_min_);
    encoder_high_wrap_ = ((encoder_max_-encoder_min_)*0.7 + encoder_min_);
    //计算里程计关键参数
    encoder_resolution_4wd_ = encoder_resolution_4wd_/linear_correction_factor_;        //脉冲数10000 / 1.0
    wheel_track_4wd_ = wheel_track_4wd_/angular_correction_factor_;                                 //轮距
    ticks_per_meter_ =( encoder_resolution_4wd_ * gear_reduction_4wd_)/(wheel_diameter_4wd_ * M_PI); //(10000 * 40)/(0.305*PI)==脉冲数/m
    dynamic_reconfigure::Server<zeus_s2_driver::PID_reconfigConfig> reconfig_server;
	dynamic_reconfigure::Server<zeus_s2_driver::PID_reconfigConfig>::CallbackType f;

	f = boost::bind(&DriverPub::dynamic_reconfig_callback,this,_1,_2);
	reconfig_server.setCallback(f);
    boost::thread recv_thread(boost::bind(&DriverPub::receive_func,this));
	sleep(1);
	if(initRobot())
    {
        //发布电机的电流0.01A、速度/rpm、位置/脉冲
        wheel_state_feedback_=n.advertise<zeus_s2_msgs::Feedback>("/motor_driverFeedback",10);
        cmd_sub_ = n.subscribe<geometry_msgs::Twist>("/zeus_speed",1,&DriverPub::cmd_vel_callback,this);      //接受上层下发的速度指令

        now_[0] = ros::Time::now();
        now_[1] = ros::Time::now();
        now_[2] = ros::Time::now();
        now_[3] = ros::Time::now();

        left_right_wheel_state_.front_left_encoder.velocity = 0;
        left_right_wheel_state_.front_right_encoder.velocity = 0;
        left_right_wheel_state_.rear_right_encoder.velocity  = 0;
        left_right_wheel_state_.rear_left_encoder.velocity  = 0;

        motorActSpeed[0] = 0;
        motorActSpeed[1] = 0;
        motorActSpeed[2] = 0;
        motorActSpeed[3] = 0;

        motorPreSpeed[0] = 0;
        motorPreSpeed[1] = 0;
        motorPreSpeed[2] = 0;
        motorPreSpeed[3] = 0;

        while(ros::ok())
        {     
            if(joy_flag_)/*遥控模式*/
            {
                if(ps3_control_.priority_flag)
                {
                    joy_flag_ = 0;
                    continue;
                }
                if(ps3_control_.parking_mode==1.0)/*速度模式下*/
                {
                    mode_isnot = true;
                    switching_mode = DRIVER_PARKING_MODE;
                    state_temp_ = DRIVER_SWITCHING_MODE;
                    ps3_control_.parking_mode = 0;
                }
                else if(ps3_control_.parking_mode == 2.0)//力矩模式下
                {         
                    mode_isnot = true;
                    switching_mode = DRIVER_OPERATION_MODE;
                    state_temp_ = DRIVER_SWITCHING_MODE;
                    ps3_control_.parking_mode = 0;
                
                }
                mode_switching_state_machine(switching_mode, mode_isnot);
            }
            else/*非遥控模式*/
            {    
                /*******************************************************/
                /*测试模式下，用做pid调节实验*/
                if(reset_vol){
                    left = speed_l;
                    right = -speed_l;
                if(pid_wheel_ ==1)
                {
                    motorPreSpeed[0] = motor_velocityPidCtl(0, left, left_right_wheel_state_.front_left_encoder.velocity);                  //m/s
                    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,motorPreSpeed[0]  );
                    printf("调节1号电机PID\r\n");
                }
                else if(pid_wheel_ ==2)
                {
                    motorPreSpeed[1] = motor_velocityPidCtl(1, right, left_right_wheel_state_.front_right_encoder.velocity);
                    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,motorPreSpeed[1] );
                    printf("调节2号电机PID\r\n");
                } 
                else if(pid_wheel_ ==3)
                {
                    motorPreSpeed[2] = motor_velocityPidCtl(2, right, left_right_wheel_state_.rear_right_encoder.velocity);
                    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,motorPreSpeed[2] );
                    printf("调节3号电机PID\r\n");
                } 
                else if(pid_wheel_ ==4)
                {
                    motorPreSpeed[3] = motor_velocityPidCtl(3, left, left_right_wheel_state_.rear_left_encoder.velocity);  
                    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL, motorPreSpeed[3] );
                    printf("调节4号电机PID\r\n");
                }              
                wheel_state_feedback_.publish(left_right_wheel_state_);
                ros::spinOnce();
                loop_rate.sleep();
                continue;
            }
            
        }
        send_speed_callback();              //将cmd_vel下发的速度处理后，用于发送给驱动器
        /*******************************************************/
        /*pid控制*/
        motorPreSpeed[0] = motor_velocityPidCtl(0, left,  left_right_wheel_state_. front_left_encoder.velocity);                  //m/s
        motorPreSpeed[1] = motor_velocityPidCtl(1, right , left_right_wheel_state_. front_right_encoder.velocity);
        motorPreSpeed[2] = motor_velocityPidCtl(2, right ,left_right_wheel_state_. rear_right_encoder.velocity );
        motorPreSpeed[3] = motor_velocityPidCtl(3, left,left_right_wheel_state_. rear_left_encoder.velocity );    
        motor_driver_check();                 //检测驱动器通信问题
        /*发送电机电流闭环控制*/
        Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL,    motorPreSpeed[0] );
        Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL,    motorPreSpeed[1] );
        Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL,  motorPreSpeed[2] );
        Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL,   motorPreSpeed[3] );
        wheel_state_feedback_.publish(left_right_wheel_state_);
        ++count;
        ros::spinOnce();
        loop_rate.sleep();
    }
    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
    printf("退出驱动程序！！！！！！！！！！！！！！！！！\n");
    }
}

//运动模式和驻车模式逻辑切换状态机
void DriverPub::mode_switching_state_machine(uint8_t state, bool isnot)
{
    if(isnot)
    {        
        switch (state_temp_)
        {
            case DRIVER_SWITCHING_MODE:
                /*清空接收标志变量*/
                mode_rev_flag = 0;
                /*判断进入模式，并发送对应的模式指令*/
                if(state == DRIVER_OPERATION_MODE)
                {
                    /*设定电机力矩模式*/
                    Set_motor_torque_mode(DRIVER_ID_ONE_CONTROL );
                    Set_motor_torque_mode(DRIVER_ID_TWO_CONTROL );
                    Set_motor_torque_mode(DRIVER_ID_THREE_CONTROL );
                    Set_motor_torque_mode(DRIVER_ID_FOUR_CONTROL );
                    /*设定电机的力矩为零*/
                    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
                    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
                    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
                    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
                    state_temp_ = DRIVER_TIMEOUT_DETECTION;  
                }
                if(state == DRIVER_PARKING_MODE)
                {
                    /*设置速度模式*/
                    Set_motor_speed_mode(DRIVER_ID_ONE_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_TWO_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_THREE_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_FOUR_CONTROL );
                    /*设置速度值*/
                    Set_speed_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
                    state_temp_ = DRIVER_TIMEOUT_DETECTION;
                }
                /*进入超时判断状态*/
                /*置位时间函数*/
                time_setMana(TIME_DRIVER_MODE);
                //  printf("-----------------------------------------------------------------------------\n");
                break;
            case DRIVER_OPERATION_MODE:
                /*运行模式下，灯光状态*/
                mode_isnot = false;
                state_temp_ = DRIVER_SWITCHING_MODE;
                break;
            case DRIVER_TIMEOUT_DETECTION:
                /*判断时间是否超时  8ms*100 ==400ms*/
                if(time_checkHandle(TIME_DRIVER_MODE, 100))
                {
                    /*打印错误提示，可使用灯光报警*/
                    printf("模式切换超时！！\n");
                    mode_isnot = false;
                }
                else/*未超时*/
                {
                    /*判断模式接收标志是否完成接收，完成接收，进入对应的状态模式*/
                    if(mode_rev_flag==0x0F){
                        if(state == DRIVER_OPERATION_MODE)
                        {
                            state_temp_ = DRIVER_OPERATION_MODE;
                        }
                        else if(state == DRIVER_PARKING_MODE)
                        {
                            state_temp_ = DRIVER_PARKING_MODE;
                        }
                    }
                }
                break;
            case DRIVER_PARKING_MODE:
                /*运行模式下，灯光状态*/
                mode_isnot = false;
                state_temp_ = DRIVER_SWITCHING_MODE;
                break;
            default:
                break;
        }
    }
}

/*
*       采集心跳包时间计数
*
*
*/
void  DriverPub::time_setMana(unsigned char  time_id)
{
        timeArray[time_id] = my_xTaskGetTickCount();

}

/*
*       检查时间是否达到
*       返回值：
*       1   :   时间到
*       0   ： 时间未到达
*/
unsigned char  DriverPub::time_checkHandle(unsigned char  time_id, unsigned short time_ms)
{
    if((my_xTaskGetTickCount() - timeArray[time_id]) >= time_ms){
        return 1;
    }
    else
    {
        return 0;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Driver_s2_node");
    DriverPub driver_s2_usb;
    driver_s2_usb.loop();
    return 0;
}
